#include "includes.h"
#include <string.h> 
#include <stdio.h>  
#include <stdbool.h>
#include <IOdevice.h>
#include "xrj_protocol.h"

#include "Motor.h"

//===电机电流过流检测
//==https://blog.csdn.net/weixin_43520036/article/details/103304181

UINT32 Adc_MotorList[2][6] = {
	{0,0,0,0,0,0},
	{0,0,0,0,0,0}
};

UINT32 MotorIndex[2] = {0,0};
UINT32 MotorAdcValue[2] = {0,0};
INT32 adcStart[2] = {-1,-1};
void Adc_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	ADC_InitTypeDef ADC_InitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_ADC1,ENABLE);
	
	RCC_ADCCLKConfig(RCC_PCLK2_Div4);
	
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AIN;
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1;
	GPIO_Init(GPIOA,&GPIO_InitStructure);
	
	ADC_DeInit(ADC1);
	
	ADC_InitStructure.ADC_ContinuousConvMode=ENABLE;
	ADC_InitStructure.ADC_DataAlign=ADC_DataAlign_Right;
	ADC_InitStructure.ADC_ExternalTrigConv=ADC_ExternalTrigConv_None;
	ADC_InitStructure.ADC_Mode=ADC_Mode_Independent;
	ADC_InitStructure.ADC_NbrOfChannel=1;
	ADC_InitStructure.ADC_ScanConvMode=DISABLE;
	ADC_Init(ADC1,&ADC_InitStructure);
	
	ADC_Cmd(ADC1,ENABLE);
	ADC_ResetCalibration(ADC1);
	while(ADC_GetResetCalibrationStatus(ADC1));
	ADC_StartCalibration(ADC1);
	while(ADC_GetCalibrationStatus(ADC1));
		

}

UINT16 Get_Adc(UINT32 id)
{
	UINT8 ch = ADC_Channel_0;
	
	if (id == 1)
	{
		ch = ADC_Channel_1;
	}
	
	ADC_RegularChannelConfig(ADC1,ch,1,ADC_SampleTime_1Cycles5);
	ADC_RegularChannelConfig(ADC1,ch,2,ADC_SampleTime_1Cycles5);
	ADC_SoftwareStartConvCmd(ADC1,ENABLE);
	while(!ADC_GetFlagStatus(ADC1,ADC_FLAG_EOC));
	return ADC_GetConversionValue(ADC1);
}
void Get_Adc_Average(UINT32 id)
{
	UINT32 temp1=0;
	
	Adc_MotorList[id][MotorIndex[id]] = Get_Adc(id);
	
	MotorIndex[id]++;

	if (MotorIndex[id]>=6)
	{
		MotorIndex[id] = 0;
		for (int i =1; i<6; i++)
		{
			for (int j=0; j<6-i;j++)
			{
				UINT32 temp = 0;
				if (Adc_MotorList[id][j]>Adc_MotorList[id][j+1])
				{
					temp = Adc_MotorList[id][j];
					Adc_MotorList[id][j] = Adc_MotorList[id][j+1];
					Adc_MotorList[id][j+1] = temp;
				}
			}
		}
		//==取中三个值做平均
		for (int k = 1; k<5; k++)
		{
			temp1 += Adc_MotorList[id][k];
		}
		MotorAdcValue[id] = temp1/4;
	}
}

void  ADC_Start(UINT32 id)
{
	adcStart[id] = 0;
	MotorIndex[id] = 0;
	MotorAdcValue[id] = 0;

	for (int j=0;j<6;j++)
	{
		Adc_MotorList[id][j] = 0;
	}
}	

void ADC_Stop(UINT32 id)
{
	adcStart[id] = -1;
}
//--------电机控制-------------------------------------------------------------------------------------------


#define	GPIO_Pin_Motor1_P		GPIO_Pin_9
#define	GPIO_Pin_Motor1_N		GPIO_Pin_8

#define	GPIO_Pin_Motor2_P		GPIO_Pin_4
#define	GPIO_Pin_Motor2_N		GPIO_Pin_3


#define	P_Out_Motor1_P	 		PBout(9)
#define	P_Out_Motor1_N	 		PBout(8)

#define	P_Out_Motor2_P	 		PBout(4)
#define	P_Out_Motor2_N	 		PBout(3)



enum {
	Motor1_ID = 0,
	Motor2_ID =	1,	
};

typedef struct _st_Motor_Manage{
	UINT32 MotorCmd;	//==收到命令
	
	UINT32 ResetCmd;	//==电机卡住后，反向复位命令
	UINT32 ResetCnt;	//==复位次数
	
	UINT32 RunCnt;	
	UINT32 RunStatus;
	
	UINT32 StopCnt;
	UINT32 StopStatus;
}st_Motor_Manage;
enum{
	Motor_Stop = 0,
	Motor_Foreward = 1,
	Motor_Reversal = 2,
};
enum{
	MOTOR_STOP_DIS   = 0,
	MOTOR_STOP_EN    = 1,
	MOTOR_STOP_DOING = 2,
};

#define	D_STOP_DELAY 	30
#define	D_RUN_DELAY 	500


#define STEP_RUN		(0)
#define STEP_STOP2		(STEP_RUN+D_RUN_DELAY)
#define STEP_END		(STEP_STOP2+D_STOP_DELAY)



st_Motor_Manage Motor_Manage[2] = 
{
	{Motor_Stop, Motor_Stop, 0, 0, Motor_Stop, 0, MOTOR_STOP_DIS},
	{Motor_Stop, Motor_Stop, 0, 0, Motor_Stop, 0, MOTOR_STOP_DIS},
};


void MOTOR_Init(void)
{				
	GPIO_InitTypeDef  GPIO_InitStructure; 
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE); //原来的初始化代码没有打开时钟 
	
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable , ENABLE);
	
	GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_Motor1_P|GPIO_Pin_Motor1_N|GPIO_Pin_Motor2_P|GPIO_Pin_Motor2_N; 
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;     
	GPIO_Init(GPIOB, &GPIO_InitStructure); 	
		
	P_Out_Motor1_P = 0;
	P_Out_Motor1_N = 0;
	
	P_Out_Motor2_P = 0;
	P_Out_Motor2_N = 0;
	
	Adc_Init();
}


void MOTOR_Foreward(UINT32 id)
{
	if (id == Motor1_ID)
	{
		P_Out_Motor1_N = 0;
		P_Out_Motor1_P = 1;	
	}	
	else if (id == Motor2_ID)
	{
		P_Out_Motor2_N = 0;
		P_Out_Motor2_P = 1;		
	}
	else
	{
		return;
	}	
	Motor_Manage[id].RunStatus = Motor_Foreward;
	Motor_Manage[id].StopStatus = MOTOR_STOP_EN;
	ADC_Start(id);
}	


void MOTOR_Reversal(UINT32 id)
{
	if (id == Motor1_ID)
	{
		P_Out_Motor1_P = 0;
		P_Out_Motor1_N = 1;
	}	
	else if (id == Motor2_ID)
	{	
		P_Out_Motor2_P = 0;
		P_Out_Motor2_N = 1;
	}
	else
	{
		return;
	}	
	Motor_Manage[id].RunStatus = Motor_Reversal;
	Motor_Manage[id].StopStatus = MOTOR_STOP_EN;
	ADC_Start(id);
}	

void MOTOR_Stop(UINT32 id)
{
	if (id == Motor1_ID)
	{
		P_Out_Motor1_N = 0;
		P_Out_Motor1_P = 0;
	}	
	else if (id == Motor2_ID)
	{	
		P_Out_Motor2_N = 0;
		P_Out_Motor2_P = 0;
	}
	else
	{
		return;
	}	
	Motor_Manage[id].RunStatus = Motor_Stop;
	ADC_Stop(id);
}	
	
void MOTOR_StopInit(UINT32 id)
{
	if (Motor_Manage[id].StopStatus == MOTOR_STOP_EN)
	{
		Motor_Manage[id].StopStatus = MOTOR_STOP_DOING;
		Motor_Manage[id].StopCnt = 0;
		MOTOR_Stop(id);
	}	
}	
void SetPlaneReport(UINT32 id, Iot__StatusCodes status)
{
	XRJ_mutex_lock(&Device_Value.Lock);	
	if (id == 0){	
		Device_Value.PLATE_1_Report = status;
	}else{
		Device_Value.PLATE_2_Report = status;	
	}
	XRJ_mutex_unlock(&Device_Value.Lock);
}



INT32 GetMotorError(UINT32 id)
{
	//==adc未开启
	if (adcStart[id] == 0)
	{
		Get_Adc_Average(id);
		//0xe8b 4安的电流   值 = ((电流/2)/3.3*0x1000)
		if (MotorAdcValue[id]>0xddd)
		{
			return -1;
		}
		
	}
		
	return 0;
	
}

void Send_Report_PLATE_Message(UINT32 id)
{
	if (id==0){
		Send_Report_Message(IOT__UNIT_INDEX__PLATE_1,NULL);
	}else{
		Send_Report_Message(IOT__UNIT_INDEX__PLATE_2,NULL);
	}
}


void Motor_Proc(void)
{
	UINT32 PLATE_Setting[2]; 
	UINT32 i;
	

	
	XRJ_mutex_lock(&Device_Value.Lock);	
	PLATE_Setting[0] = Device_Value.PLATE_1_Setting; 
	PLATE_Setting[1] = Device_Value.PLATE_2_Setting; 
	Device_Value.PLATE_1_Setting = IOT__STATUS_CODES__NONE;
	Device_Value.PLATE_2_Setting = IOT__STATUS_CODES__NONE;
	XRJ_mutex_unlock(&Device_Value.Lock);
	//===命令部分
	for (i = 0; i < 2; i++)
	{	
		if (PLATE_Setting[i] == IOT__STATUS_CODES__ON)
		{
			//if (Motor_Manage[i].MotorCmd != Motor_Foreward)
			{	//===执行开门命令
				Motor_Manage[i].MotorCmd = Motor_Foreward;
				Motor_Manage[i].ResetCmd = Motor_Stop;
				Motor_Manage[i].ResetCnt = 0;		
				Motor_Manage[i].RunCnt = 0;	
				
				MOTOR_StopInit(i);
			}	
		}	

		if (PLATE_Setting[i] == IOT__STATUS_CODES__OFF)
		{
			//if (Motor_Manage[i].MotorCmd != Motor_Reversal)
			{	//===执行关门命令
				Motor_Manage[i].MotorCmd = Motor_Reversal;
				Motor_Manage[i].ResetCmd = Motor_Stop;
				Motor_Manage[i].ResetCnt = 0;
				Motor_Manage[i].RunCnt = 0;	
				
				MOTOR_StopInit(i);
			}	
		}		
	}	
	//===执行部分
	for (i = 0; i < 2; i++)
	{
		UINT32 tStepStop =  0;
		if (i==0){
			tStepStop = Device_Value.PLATE_1_Setting_Time*100;
		}else{
			tStepStop = Device_Value.PLATE_2_Setting_Time*100;
		}
		
		if (Motor_Manage[i].StopStatus == MOTOR_STOP_DOING)
		{//===电机启动前，需停止一段时间
			Motor_Manage[i].StopCnt++;
			if (Motor_Manage[i].StopCnt >= D_STOP_DELAY)		
			{
				Motor_Manage[i].StopStatus = MOTOR_STOP_DIS;
			}			
		} 
		else if ((Motor_Manage[i].ResetCmd == Motor_Foreward)||  (Motor_Manage[i].ResetCmd == Motor_Reversal))
		{ //===卡住后方向复位流程
			UINT32 tStepEnd = tStepStop+ 100*10;
			INT32 isEnd = -1;
			if (GetMotorError(i) == 0){
				if (Motor_Manage[i].RunCnt == STEP_RUN)
				{
					if (Motor_Manage[i].MotorCmd == Motor_Reversal)	
					{
						MOTOR_Foreward(i);										
					}
					//===
//					else
//					{
//						MOTOR_Reversal(i);
//					}				
				}	
				else if (Motor_Manage[i].RunCnt >= tStepEnd) // loop=10ms
				{ // 停留30秒后，返回
					Motor_Manage[i].RunCnt = 0;	
					Motor_Manage[i].ResetCmd = Motor_Stop;
					isEnd = 0;
				}	
				else if (Motor_Manage[i].RunCnt >= tStepStop) 
				{ //==5秒停
					MOTOR_StopInit(i);				
				}
				
				if (isEnd != 0){
					Motor_Manage[i].RunCnt++;
				}	
			}else{
				MOTOR_StopInit(i);
				if (Motor_Manage[i].RunCnt >= tStepEnd) // loop=10ms
				{ // 停留30秒后，返回
					Motor_Manage[i].RunCnt = 0;	
					Motor_Manage[i].ResetCmd = Motor_Stop;
				}else{
					Motor_Manage[i].RunCnt++;
				}	
			}
		}	
		else if ((Motor_Manage[i].MotorCmd == Motor_Foreward) ||  (Motor_Manage[i].MotorCmd == Motor_Reversal))
		{//===正常流程		
			//===电机是否异常，0正常
			if (GetMotorError(i) == 0){
				if (Motor_Manage[i].RunCnt == STEP_RUN)
				{
					if (Motor_Manage[i].MotorCmd == Motor_Foreward)	{
						MOTOR_Foreward(i);	
					}
					else{
						MOTOR_Reversal(i);
					}
					SetPlaneReport(i, IOT__STATUS_CODES__DOING);				
				}	
				else if (Motor_Manage[i].RunCnt >= tStepStop)//STEP_STOP2
				{
					MOTOR_StopInit(i);
					Motor_Manage[i].MotorCmd = Motor_Stop;

					if (Motor_Manage[i].MotorCmd == Motor_Foreward){
						SetPlaneReport(i, IOT__STATUS_CODES__ON);					
					}else{
						SetPlaneReport(i, IOT__STATUS_CODES__OFF);
					}

					Send_Report_PLATE_Message(i);	
				}	
				Motor_Manage[i].RunCnt++;	
				
			}else{
				SetPlaneReport(i, IOT__STATUS_CODES__ANOMALY);
				if (Motor_Manage[i].MotorCmd == Motor_Foreward){
					Motor_Manage[i].ResetCmd = Motor_Reversal;
				}else{
					Motor_Manage[i].ResetCmd = Motor_Foreward;
				}
				Motor_Manage[i].ResetCnt++;
				Motor_Manage[i].RunCnt = 0;			
				MOTOR_StopInit(i);
				//==上报一个异常
				if (Motor_Manage[i].ResetCnt>3)
				{
					Send_Report_PLATE_Message(i);
				}
			}			
		}		
	}	
	
//	for (i = 0; i < 2; i++)
//	{
//		if (Motor_Manage[i].MotorCmd == Motor_Foreward)
//		{	
//			if (PLATE_Report[i] == IOT__STATUS_CODES__ON)
//			{
//				MOTOR_StopInit(i);
//				Motor_Manage[i].MotorCmd = Motor_Stop;				
//			}	
//		}	
//		
//		if (Motor_Manage[i].MotorCmd == Motor_Reversal)
//		{	
//			if (PLATE_Report[i] == IOT__STATUS_CODES__OFF)
//			{
//				MOTOR_StopInit(i);
//				Motor_Manage[i].MotorCmd = Motor_Stop;				
//			}	
//		}	
//	}

		
}	




#define	GPIO_Pin_FAN		GPIO_Pin_8


#define	P_Out_FAN	 		PAout(8)




void FAN_Init(void)
{				
	GPIO_InitTypeDef  GPIO_InitStructure; 
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO, ENABLE); //原来的初始化代码没有打开时钟 
	

	
	GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_FAN; 
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;     
	GPIO_Init(GPIOA, &GPIO_InitStructure); 	
	P_Out_FAN = 0;	
}

void FAN_Open(void)
{
	P_Out_FAN = 1;
}	

void FAN_Close(void)
{
	P_Out_FAN = 0;
}

void FAN_PROC(void)
{
	Iot__StatusCodes	Fan_Set;
	XRJ_mutex_lock(&Device_Value.Lock);	
	Fan_Set = Device_Value.FAN_Setting; 
 	Device_Value.FAN_Setting = IOT__STATUS_CODES__NONE;
	XRJ_mutex_unlock(&Device_Value.Lock);

	if (Fan_Set == IOT__STATUS_CODES__ON)
	{
		FAN_Open();
	}
	else if (Fan_Set == IOT__STATUS_CODES__OFF)
	{
		FAN_Close();	
	}	
}	






#define	GPIO_Pin_LOCK_1		GPIO_Pin_0
#define	GPIO_Pin_LOCK_2		GPIO_Pin_2

#define	P_Out_LOCK_1	 		PCout(0)
#define	P_Out_LOCK_2	 		PCout(2)
#define	P_Out_SW1	 	 		PCout(4)

void LOCK_Init(void)
{				
	GPIO_InitTypeDef  GPIO_InitStructure; 
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE); //原来的初始化代码没有打开时钟 
	

	
	GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_LOCK_1|GPIO_Pin_LOCK_2; 
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;     
	GPIO_Init(GPIOC, &GPIO_InitStructure); 	
	//默认开
//	P_Out_LOCK_1 = 1;	
//	P_Out_LOCK_2 = 1;
}
void LOCK_Open(UINT32 index)
{
	
	if (index == IOT__UNIT_INDEX__LOCK_1)
	{
		printf("lock1 open\n");
		P_Out_LOCK_1 = 1;
	}
	else if(index == IOT__UNIT_INDEX__LOCK_2)
	{
		printf("lock2 open\n");
		P_Out_LOCK_2 = 1;
	}	
	else if (index == IOT__UNIT_INDEX__LIGHT_INSIDE_1)
	{
		P_Out_SW1 = 1;
	}
}


void LOCK_Close(UINT32 index)
{
	
	if (index == IOT__UNIT_INDEX__LOCK_1)
	{
		P_Out_LOCK_1 = 0;
		printf("lock1 close\n");
	}
	else if(index == IOT__UNIT_INDEX__LOCK_2)
	{
		P_Out_LOCK_2 = 0;
		printf("lock2 close\n");
	}	
	else if (index == IOT__UNIT_INDEX__LIGHT_INSIDE_1)
	{
		P_Out_SW1 = 0;
	}
}

UINT32 Lock_Mode1 = 0;
UINT32 Lock_Cnt1 = 0;
UINT32 Lock_Mode2 = 0;
UINT32 Lock_Cnt2 = 0;
UINT32 Lock_Mode3 = 0;
UINT32 Lock_Cnt3 = 0;

#define	D_LOCK_TIME		50

void LOCK_PROC(void)
{
	
	Iot__StatusCodes	LOCK_Set_1;
	Iot__StatusCodes	LOCK_Set_2;
	Iot__StatusCodes	LOCK_Set_3;
	
	XRJ_mutex_lock(&Device_Value.Lock);	
	LOCK_Set_1 = Device_Value.LOCK_1_Setting;
	LOCK_Set_2 = Device_Value.LOCK_2_Setting;	
	LOCK_Set_3 = Device_Value.LIGHT_INSIDE_1_Setting;
 	Device_Value.LOCK_1_Setting = IOT__STATUS_CODES__NONE;
	Device_Value.LOCK_2_Setting = IOT__STATUS_CODES__NONE;
	Device_Value.LIGHT_INSIDE_1_Setting = IOT__STATUS_CODES__NONE;
	XRJ_mutex_unlock(&Device_Value.Lock);

	if (LOCK_Set_1 == IOT__STATUS_CODES__ON)
	{
		if (Lock_Mode1 == 0)
		{
			Lock_Mode1 = 1;
			Lock_Cnt1 = 0;
			LOCK_Open(IOT__UNIT_INDEX__LOCK_1);
		}		
	}
	if (Lock_Mode1 != 0)
	{
		Lock_Cnt1++;
		if (Lock_Cnt1 > D_LOCK_TIME)
		{
			Lock_Mode1 = 0;
			LOCK_Close(IOT__UNIT_INDEX__LOCK_1);	
		}		
	}
	
	
	
	if (LOCK_Set_2 == IOT__STATUS_CODES__ON)
	{
		if (Lock_Mode2 == 0)
		{
			Lock_Mode2 = 1;
			Lock_Cnt2 = 0;
			LOCK_Open(IOT__UNIT_INDEX__LOCK_2);
		}		
	}
	if (Lock_Mode2 != 0)
	{
		Lock_Cnt2++;
		if (Lock_Cnt2 > D_LOCK_TIME)
		{
			Lock_Mode2 = 0;
			LOCK_Close(IOT__UNIT_INDEX__LOCK_2);	
		}		
	}
	
	if (LOCK_Set_3 == IOT__STATUS_CODES__ON)
	{
		if (Lock_Mode3 == 0)
		{
			Lock_Mode3 = 1;
			Lock_Cnt3 = 0;
			LOCK_Open(IOT__UNIT_INDEX__LIGHT_INSIDE_1);
		}		
	}
	if (Lock_Mode3 != 0)
	{
		Lock_Cnt3++;
		if (Lock_Cnt3 > D_LOCK_TIME)
		{
			Lock_Mode3 = 0;
			LOCK_Close(IOT__UNIT_INDEX__LIGHT_INSIDE_1);	
		}		
	}


}	











